#include "octree.h"
#include "function.h"
#include <bitset>
OctreeNode::OctreeNode(const Point& center, const float& halflength, const float& halfwidth, const float& halfheight)
{
    m_center = center;
    m_halfwidth = halfwidth;
    m_halfheight = halfheight;
    m_halflength = halflength;
    m_R2 = static_cast<float>(sqrt(halfwidth * halfwidth + halflength * halflength));
    m_R3 = static_cast<float>(sqrt(halfwidth * halfwidth + halflength * halflength + halfheight * halfheight));
    m_scenepointslist = nullptr;
    m_obstaclepointslist = nullptr;
    m_pointnum = 0;
    ChildExists = 0;
    m_noattributepointslist = {};
    for (int i = 0; i < 8; ++i) {
        m_children[i] = nullptr;
    }
};

OctreeNode* OctreeNode::BuildOctree(OctreeNode* parent,const Point& center, const float& halflength, const float& halfwidth, const float& halfheight, int stopDepth) {
    if (stopDepth < 0) return nullptr;
    else {
        // Construct and fill in 'root' of this subtree
        OctreeNode* pNode = new OctreeNode();
        pNode->m_center = center;
        pNode->m_halfwidth = halfwidth;
        pNode->m_halfheight = halfheight;
        pNode->m_halflength = halflength;
        pNode->m_R2 = static_cast<float>(pow(halfwidth * halfwidth + halflength * halflength,0.5));
        pNode->m_R3 = static_cast<float>(pow(halfwidth * halfwidth + halflength * halflength + halfheight * halfheight,0.5));
        pNode->m_scenepointslist = nullptr;
        pNode->m_obstaclepointslist = nullptr;
        pNode->m_pointnum = 0;
        pNode->m_noattributepointslist = {};
        pNode->m_parent = parent;

        //递归的构造出他的八个子节点
        Point offset;
        float widthstep = halfwidth * 0.5f;
        float heightstep = halfheight * 0.5f;
        float lengthstep = halflength * 0.5f;
        for (int i = 0; i < 8; ++i) {
            // 子节点对应中心位置和父节点中心位置的偏移量
            offset.m_x = ((i & 1) ? lengthstep : -lengthstep);
            offset.m_y = ((i & 2) ? widthstep : -widthstep);
            offset.m_z = ((i & 4) ? heightstep : -heightstep);
            Point realcenter = center + offset;
            pNode->m_children[i] = BuildOctree(pNode,realcenter, lengthstep, widthstep, heightstep, stopDepth - 1);
        }
        return pNode;
    }
}

void OctreeNode::InsertObjectToScene(OctreeNode* pTree, Point* pPoint) {
    //点在不在矩形里面，只需要判断点的坐标和矩形中心及其长宽高的关系
    //以长为例，只要点的x 和 矩形中心的 x 的差的绝对值小于等于 矩形的半长，则在 x 方向上，此点落在矩形中。
    if(pPoint == nullptr || pTree == nullptr){
        std::cerr<<"point/octreenode isnot empty!";
        return;
    }

    int straddle = 0, index = 0;

    for (int i = 0; i < 3; i++)
    {
        float delta = (*pPoint)[i] - pTree->m_center[i];
        //说明点没有落在该矩形内
        if (abs(delta) > (*pTree)[i])
        {
            straddle = 1;
            break;
        }
        //判断落点应该在那个子节点上
        if (delta > 0.0f)
            index |= (1 << i);
    }
    if(!straddle){
        //点落在区域内且子节点不为空则递归的调用，一直找到该点所在的最小子块
        if (pTree->m_children[index]) {
            InsertObjectToScene(pTree->m_children[index], pPoint);
        }
        //如果这是一个叶子节点含有元素且未到达终止条件,构造下一层并插入,而且还需要把父节点的点移动到子节点上。
        else if(pTree->m_pointnum != 0 && GTE(pTree->m_halfheight,0.2)){
            //递归的构造出他的八个子节点
            Point offset;
            float widthstep = pTree->m_halfwidth * 0.5f;
            float heightstep = pTree->m_halfheight * 0.5f;
            float lengthstep = pTree->m_halflength * 0.5f;
            for (int i = 0; i < 8; ++i) {
                // 子节点对应中心位置和父节点中心位置的偏移量
                offset.m_x = ((i & 1) ? lengthstep : -lengthstep);
                offset.m_y = ((i & 2) ? widthstep : -widthstep);
                offset.m_z = ((i & 4) ? heightstep : -heightstep);
                Point realcenter = pTree->m_center + offset;
                pTree->m_children[i] = new OctreeNode(realcenter, lengthstep, widthstep, heightstep);
                pTree->m_children[i]->m_parent = pTree;
            }
            //插入,问题在于如果重插入之后发现两个点还是在一个节点内，将引起致命错误
            --pTree->m_pointnum;
            InsertObjectToScene(pTree->m_children[index], pPoint);
            if(pTree->m_scenepointslist){
                int idx = 0;
                for (int i = 0; i < 3; i++)
                {
                    float delta = (*pTree->m_scenepointslist)[i] - pTree->m_center[i];
                    //判断落点应该在那个子节点上
                    if (delta > 0.0f)
                        idx |= (1 << i);
                }
                InsertObjectToScene(pTree->m_children[idx], pTree->m_scenepointslist);
                pTree->m_scenepointslist = nullptr;
            }
            else if(!pTree->m_noattributepointslist.empty()){
                int idx = 0;
                for (int i = 0; i < 3; i++)
                {
                    float delta = (*pTree->m_noattributepointslist.front())[i] - pTree->m_center[i];
                    //判断落点应该在那个子节点上
                    if (delta > 0.0f)
                        idx |= (1 << i);
                }
                InsertObjectTonoattribute(pTree->m_children[idx], pTree->m_noattributepointslist.front());
                pTree->m_noattributepointslist.clear();
            }
            else{
                int idx = 0;
                for (int i = 0; i < 3; i++)
                {
                    float delta = (*pTree->m_obstaclepointslist)[i] - pTree->m_center[i];
                    //判断落点应该在那个子节点上
                    if (delta > 0.0f)
                        idx |= (1 << i);
                }
                InsertObjectToObstacle(pTree->m_children[idx],pTree->m_obstaclepointslist);
                pTree->m_obstaclepointslist = nullptr;
            }

        }
        else{
            //没有子节点并且不含有节点直接插入当前节点的队列中去，头插法
            pPoint->m_next = pTree->m_scenepointslist;
            pTree->m_scenepointslist = pPoint;
            ++(pTree->m_pointnum);
        }
    }
    else
        InsertObjectToScene(pTree->m_parent, pPoint);
}
void OctreeNode::InsertObjectToObstacle(OctreeNode* pTree, Point* pPoint){
    if(pPoint == nullptr || pTree == nullptr){
        std::cerr<<"point/octreenode isnot empty!";
        return;
    }
    int straddle = 0, index = 0;

    for (int i = 0; i < 3; i++)
    {
        float delta = (*pPoint)[i] - pTree->m_center[i];
        //说明点没有落在该矩形内
        if (abs(delta) > (*pTree)[i])
        {
            straddle = 1;
            break;
        }
        //判断落点应该在那个子节点上
        if (delta > 0.0f)
            index |= (1 << i);
    }
    if(!straddle){
        //点落在区域内且子节点不为空则递归的调用，一直找到该点所在的最小子块
        if (pTree->m_children[index]) {
            InsertObjectToObstacle(pTree->m_children[index], pPoint);
        }
        //如果这是一个叶子节点并且含有元素,构造下一层并插入
        else if(pTree->m_pointnum != 0 && GTE(pTree->m_halfheight,0.02)){
            //递归的构造出他的八个子节点
            Point offset;
            float widthstep = pTree->m_halfwidth * 0.5f;
            float heightstep = pTree->m_halfheight * 0.5f;
            float lengthstep = pTree->m_halflength * 0.5f;
            for (int i = 0; i < 8; ++i) {
                // 子节点对应中心位置和父节点中心位置的偏移量
                offset.m_x = ((i & 1) ? lengthstep : -lengthstep);
                offset.m_y = ((i & 2) ? widthstep : -widthstep);
                offset.m_z = ((i & 4) ? heightstep : -heightstep);
                Point realcenter = pTree->m_center + offset;
                pTree->m_children[i] = new OctreeNode(realcenter, lengthstep, widthstep, heightstep);
                pTree->m_children[i]->m_parent = pTree;
            }
            //插入
            --pTree->m_pointnum;
            InsertObjectToObstacle(pTree->m_children[index], pPoint);
            if(pTree->m_scenepointslist){
                int idx = 0;
                for (int i = 0; i < 3; i++)
                {
                    float delta = (*pTree->m_scenepointslist)[i] - pTree->m_center[i];
                    //判断落点应该在那个子节点上
                    if (delta > 0.0f)
                        idx |= (1 << i);
                }
                InsertObjectToScene(pTree->m_children[idx], pTree->m_scenepointslist);
                pTree->m_scenepointslist = nullptr;
            }
            else if(!pTree->m_noattributepointslist.empty()){
                int idx = 0;
                for (int i = 0; i < 3; i++)
                {
                    float delta = (*pTree->m_noattributepointslist.front())[i] - pTree->m_center[i];
                    //判断落点应该在那个子节点上
                    if (delta > 0.0f)
                        idx |= (1 << i);
                }
                InsertObjectTonoattribute(pTree->m_children[idx], pTree->m_noattributepointslist.front());
                pTree->m_noattributepointslist.clear();
            }
            else{
                int idx = 0;
                for (int i = 0; i < 3; i++)
                {
                    float delta = (*pTree->m_obstaclepointslist)[i] - pTree->m_center[i];
                    //判断落点应该在那个子节点上
                    if (delta > 0.0f)
                        idx |= (1 << i);
                }
                InsertObjectToObstacle(pTree->m_children[idx],pTree->m_obstaclepointslist);
                pTree->m_obstaclepointslist = nullptr;
            }
        }
        else {
            //没有子节点并且不含有节点直接插入当前节点的队列中去，头插法
            pPoint->m_next = pTree->m_obstaclepointslist;
            pTree->m_obstaclepointslist = pPoint;
            ++(pTree->m_pointnum);
        }
    }
    else
        InsertObjectToScene(pTree->m_parent, pPoint);
}
void OctreeNode::InsertObjectTonoattribute(OctreeNode* pTree, Point* pPoint){
    if(pPoint == nullptr || pTree == nullptr){
        std::cerr<<"point/octreenode isnot empty!";
        return;
    }
    int straddle = 0, index = 0;

    for (int i = 0; i < 3; i++)
    {
        float delta = (*pPoint)[i] - pTree->m_center[i];
        //说明点没有落在该矩形内
        if (abs(delta) > (*pTree)[i])
        {
            straddle = 1;
            break;
        }
        //判断落点应该在那个子节点上
        if (delta > 0.0f)
            index |= (1 << i);
    }
    //如果点落在区域内
    if(!straddle){
        //点落在区域内且子节点不为空则递归的调用，一直找到该点所在的最小子块
        if (pTree->m_children[index]) {
            InsertObjectTonoattribute(pTree->m_children[index], pPoint);
        }
        //如果这是一个叶子节点并且含有元素,构造下一层并插入
        else if(pTree->m_pointnum != 0 && GTE(pTree->m_halfheight,0.02)){
            //递归的构造出他的八个子节点
            Point offset;
            float widthstep = pTree->m_halfwidth * 0.5f;
            float heightstep = pTree->m_halfheight * 0.5f;
            float lengthstep = pTree->m_halflength * 0.5f;
            for (int i = 0; i < 8; ++i) {
                // 子节点对应中心位置和父节点中心位置的偏移量
                offset.m_x = ((i & 1) ? lengthstep : -lengthstep);
                offset.m_y = ((i & 2) ? widthstep : -widthstep);
                offset.m_z = ((i & 4) ? heightstep : -heightstep);
                Point realcenter = pTree->m_center + offset;
                pTree->m_children[i] = new OctreeNode(realcenter, lengthstep, widthstep, heightstep);
                pTree->m_children[i]->m_parent = pTree;
            }
            //插入
            --pTree->m_pointnum;
            InsertObjectTonoattribute(pTree->m_children[index], pPoint);
            if(pTree->m_scenepointslist){
                int idx = 0;
                for (int i = 0; i < 3; i++)
                {
                    float delta = (*pTree->m_scenepointslist)[i] - pTree->m_center[i];
                    //判断落点应该在那个子节点上
                    if (delta > 0.0f)
                        idx |= (1 << i);
                }
                InsertObjectToScene(pTree->m_children[idx], pTree->m_scenepointslist);
                pTree->m_scenepointslist = nullptr;
            }
            else if(!pTree->m_noattributepointslist.empty()){
                int idx = 0;
                for (int i = 0; i < 3; i++)
                {
                    float delta = (*pTree->m_noattributepointslist.front())[i] - pTree->m_center[i];
                    //判断落点应该在那个子节点上
                    if (delta > 0.0f)
                        idx |= (1 << i);
                }
                InsertObjectTonoattribute(pTree->m_children[idx], pTree->m_noattributepointslist.front());
                pTree->m_noattributepointslist.clear();
            }
            else{
                int idx = 0;
                for (int i = 0; i < 3; i++)
                {
                    float delta = (*pTree->m_obstaclepointslist)[i] - pTree->m_center[i];
                    //判断落点应该在那个子节点上
                    if (delta > 0.0f)
                        idx |= (1 << i);
                }
                InsertObjectToObstacle(pTree->m_children[idx],pTree->m_obstaclepointslist);
                pTree->m_obstaclepointslist = nullptr;
            }
        }
        else {
            //没有子节点并且不含有节点直接插入当前节点的队列中去，头插法
            pTree->m_noattributepointslist.push_back(pPoint);
            ++(pTree->m_pointnum);
        }
    }
    else
        InsertObjectToScene(pTree->m_parent, pPoint);
}
//这个函数无限递归下去了
OctreeNode* OctreeNode::SearchPoint(OctreeNode* pTree, const Point* pPoint){
    if(pTree == nullptr || pPoint == nullptr){
        std::cerr<<"Program accesses null pointer!\n";
        return nullptr;
    }
    int straddle = 0, index = 0;
//    std::cerr<<"pTree.front = "<<pTree->m_center.m_x + pTree->m_halflength<<" "
//             <<"pTree.right = "<<pTree->m_center.m_y + pTree->m_halfwidth<<" "
//             <<"pTree.top = "<<pTree->m_center.m_z + pTree->m_halfheight<<"\n";
    //获得最近祖先
    do{
        straddle = 0;
        for (int i = 0; i < 3; i++)
        {
            float delta = (*pPoint)[i] - pTree->m_center[i];
            //说明点没有落在该矩形,寻找他的父节点
    //        std::cerr<<"point["<<i<<"]= "<<(*pPoint)[i]<<"\n";
    //        std::cerr<<"ptree["<<i<<"]= "<<(*pTree)[i]<<"\n";
    //        std::cerr<<"delta = "<<delta<<"\n";
            if (std::abs(delta) > (*pTree)[i])
            {
                straddle = 1;
                if(pTree->m_parent)
                    pTree = pTree->m_parent;
                else{
                    std::cerr<<"Node is not in zone!\n";
                    return nullptr;
                }
                break;
            }
        }

    }while(straddle == 1);
    do{
        index = 0;
        for (int i = 0; i < 3; i++)
        {
            float delta = (*pPoint)[i] - pTree->m_center[i];
            if (delta > 0.0f)
                index |= (1 << i);
        }
        if(pTree->m_children[index])
        pTree = pTree->m_children[index];
    }while(pTree->m_children[index]);
    return pTree;
}
OctreeNode::~OctreeNode() {
    while (m_scenepointslist != nullptr) {
        Point* next = m_scenepointslist->m_next;
        delete m_scenepointslist;
        m_scenepointslist = next;
    }
    while (m_obstaclepointslist != nullptr) {
        Point* next = m_obstaclepointslist->m_next;
        delete m_obstaclepointslist;
        m_obstaclepointslist = next;
    }
    auto point = m_noattributepointslist.begin();
    while(point != m_noattributepointslist.end()){
        delete *point;
        point = m_noattributepointslist.erase(point);
    }
    if (m_children[0] != nullptr) {
        for (int i = 0; i < 8; ++i) {
            delete m_children[i];
        }
    }
}


std::unordered_set<uint64_t> DATUM_FRONT = { 1,3,5,7 };
std::unordered_set<uint64_t> DATUM_BACK  = { 0,2,4,6 };
std::unordered_set<uint64_t> DATUM_BEYOND= { 4,5,6,7 };
std::unordered_set<uint64_t> DATUM_BOTTOM= { 0,1,2,3 };
std::unordered_set<uint64_t> DATUM_LEFT  = { 0,1,4,5 };
std::unordered_set<uint64_t> DATUM_RIGHT = { 2,3,6,7 };

Octree::Octree() {
    Nodes.clear();
}
//获得父母节点的方法
OctreeNode* Octree::GetParentNode(OctreeNode* node)
{
    const uint64_t locCodeParent = node->MortanCode >> 3;
    return LookupNode(locCodeParent);
}
//根据MortanCode 获取节点深度
uint64_t Octree::NodeDepth(uint64_t MortanCode) {
    for (int d = 0; MortanCode; ++d) {
        if (MortanCode == 1) return d;
        MortanCode >>= 3;
    }
    assert(0); //bad !!!!!
    return 0;
}

//遍历所有节点的方法，以node为起始节点
void Octree::VisitAll(const OctreeNode* node)
{
    for (int i = 0; i < 8; i++)
    {
        if (node->ChildExists & (1 << i))
        {
            const uint64_t locCodeChild = (node->MortanCode << 3) | i;
            const auto* child = LookupNode(locCodeChild);
            VisitAll(child);
        }
    }
}
//预先给定一个深度生成八叉树
OctreeNode* Octree::BuildOctree(OctreeNode* parent,uint64_t MortanCode, const Point& center, const float& halflength, const float& halfwidth, const float& halfheight, int stopDepth) {
    if(stopDepth < 0) return nullptr;
    // Construct and fill in 'root' of this subtree
    //节点的大小和位置信息都可以隐含在八叉树的深度中，节省空间，但是会增加计算量，暂且不采用这种方式
    //看来不采用是不行了，不设置标志位1来表示深度的话，会出现相同的Mortan码
    //当然，设1的方法相当的简单，只要令根节点的MortanCode = 1而不是0就可以了
    else{
        OctreeNode* pNode = new OctreeNode();
        pNode->m_center = center;
        pNode->m_halfwidth = halfwidth;
        pNode->m_halfheight = halfheight;
        pNode->m_halflength = halflength;
        pNode->m_R2 = static_cast<float>(pow(halfwidth * halfwidth + halflength * halflength,0.5));
        pNode->m_R3 = static_cast<float>(pow(halfwidth * halfwidth + halflength * halflength + halfheight * halfheight,0.5));
        pNode->m_scenepointslist = nullptr;
        pNode->m_obstaclepointslist = nullptr;
        pNode->m_pointnum = 0;
        pNode->m_noattributepointslist = {};
        pNode->m_parent = parent;
        pNode->ChildExists = 0;
        pNode->MortanCode = MortanCode;
        Nodes[MortanCode] = pNode;

        if (stopDepth-1>= 0) {
            //递归的构造出他的八个子节点
            Vector offset;
            float widthstep = halfwidth * 0.5f;
            float heightstep = halfheight * 0.5f;
            float lengthstep = halflength * 0.5f;
            MortanCode <<= 3;
            for (int i = 0; i < 8; ++i) {
                uint64_t MortanCode_copy = MortanCode;
                // 子节点对应中心位置和父节点中心位置的偏移量
                offset.m_x = ((i & 1) ? lengthstep : -lengthstep);
                offset.m_y = ((i & 2) ? widthstep : -widthstep);
                offset.m_z = ((i & 4) ? heightstep : -heightstep);
                //得到子节点的Mortan值
                MortanCode_copy = MortanCode | i;
                (pNode->ChildExists |= (1 << i));
                Point realcenter = center + offset;
                //创建子节点
                pNode->m_children[i] = BuildOctree(pNode,MortanCode_copy, realcenter, lengthstep, widthstep, heightstep, stopDepth-1);
            }
        }
        return pNode;
    }
}

//根据Mortan查询节点
OctreeNode* Octree::LookupNode(uint64_t locCode)
{
    const auto iter = Nodes.find(locCode);
    return (iter == Nodes.end() ? nullptr : (*iter).second);
}

void Octree::InsertObject(OctreeNode* pTree, Point* pPoint,uint64_t MortanCode) {
    if (pPoint == nullptr || pTree == nullptr) {
        std::cerr << "point/octreenode is nullptr!";
        return;
    }

    int index = 0;

    for (int i = 0; i < 3; i++)
    {
        float delta = (*pPoint)[i] - pTree->m_center[i];
        if (delta > 0.0f)
            index |= (1 << i);
    }
    MortanCode <<= 3;
    //点落在区域内且子节点不为空则递归的调用，一直找到该点所在的最小子块
    if (pTree->ChildExists & (1 << index)) {
        MortanCode |= index;
        OctreeNode* nextnode = LookupNode(MortanCode);
        InsertObject(nextnode, pPoint,MortanCode);
    }
    else if(pTree->m_scenepointslist != nullptr&& GTE(pTree->m_halfheight,0.02)){//必须设置最小分辨率，以免两个点重合导致无限分割下去。
        //递归的构造出他的八个子节点
        Point offset;
        float widthstep = pTree->m_halfwidth * 0.5f;
        float heightstep = pTree->m_halfheight * 0.5f;
        float lengthstep = pTree->m_halflength * 0.5f;
        for (int i = 0; i < 8; ++i) {
            uint64_t MortanCode_copy = MortanCode;
            // 子节点对应中心位置和父节点中心位置的偏移量
            offset.m_x = ((i & 1) ? lengthstep : -lengthstep);
            offset.m_y = ((i & 2) ? widthstep : -widthstep);
            offset.m_z = ((i & 4) ? heightstep : -heightstep);
            Point realcenter = pTree->m_center + offset;
            MortanCode_copy = MortanCode | i;
            (pTree->ChildExists |= (1 << i));
            pTree->m_children[i] = new OctreeNode(realcenter, lengthstep, widthstep, heightstep);
            pTree->m_children[i]->m_parent = pTree;
            pTree->m_children[i]->MortanCode = MortanCode_copy;
            Nodes[MortanCode_copy] = pTree->m_children[i];
        }
        //插入
        uint64_t MortanCodeParent = MortanCode;
        MortanCode |= index;
        InsertObject(pTree->m_children[index], pPoint,MortanCode);
        if(pTree->m_scenepointslist){
            int idx = 0;
            for (int i = 0; i < 3; i++)
            {
                float delta = (*pTree->m_scenepointslist)[i] - pTree->m_center[i];
                //判断落点应该在那个子节点上
                if (delta > 0.0f)
                    idx |= (1 << i);
            }
            MortanCodeParent |= idx;
            InsertObject(pTree->m_children[idx], pTree->m_scenepointslist,MortanCodeParent);
            pTree->m_scenepointslist = nullptr;
        }
    }
    else {
        //没有子节点并且不含有节点直接插入当前节点的队列中去，头插法
        pPoint->m_next = pTree->m_scenepointslist;
        pTree->m_scenepointslist = pPoint;
    }
}
//MortanCode
//BzByBx
uint64_t Octree::SearchNodeByMortanCode(direction_set direction, uint64_t MortanCode,const Point* incidentpoint) {
    uint64_t bit = 0;
    uint64_t depth = NodeDepth(MortanCode);
    uint64_t i = 0;
    uint64_t threebits = 0;
    switch (direction)
    {
    case direction_set::FRONT:
        bit = 0;
        reversebit(MortanCode, bit);
        threebits = getDatum(MortanCode);
        while (DATUM_FRONT.find(threebits) == DATUM_FRONT.end()) {
            if (++i >= depth) {
                return 0;
            }
            bit += 3;
            reversebit(MortanCode, bit);
            threebits = getDatum(MortanCode >> bit); //该运算传入MortanCode的副本，不改变MortanCode的大小
        }
        break;
    case direction_set::BACK:
        bit = 0;
        reversebit(MortanCode, bit);
        threebits = getDatum(MortanCode);
        //如果这是最后一层，没找到的话就说明不在该区域内，再向上找的话就导致标志NodeDepth的标志位被改变了
        while (DATUM_BACK.find(threebits) == DATUM_BACK.end()) {
            if (++i >= depth) {
                return 0;
            }
            bit += 3;
            reversebit(MortanCode, bit);
            threebits = getDatum(MortanCode >> bit); //该运算传入MortanCode的副本，不改变MortanCode的大小
        }
        break;
    case direction_set::BOTTOM:
        bit = 2;
        reversebit(MortanCode, bit);
        threebits = getDatum(MortanCode);
        while (DATUM_BOTTOM.find(threebits) == DATUM_BOTTOM.end()) { //后三位不符合基准向，就对他的父节点0-1互换，知道符合基准向
            if (++i >= depth) {
                return 0;
            }
            bit += 3;
            reversebit(MortanCode, bit);
            threebits = getDatum(MortanCode >> (bit-2)); //该运算传入MortanCode的副本，不改变MortanCode的大小
        }
        break;
    case direction_set::BEYOND:
        bit = 2;
        reversebit(MortanCode, bit);
        threebits = getDatum(MortanCode);
        while (DATUM_BEYOND.find(threebits) == DATUM_BEYOND.end()) {
            if (++i >= depth) {
                return 0;
            }
            bit += 3;
            reversebit(MortanCode, bit);
            threebits = getDatum(MortanCode >> (bit-2)); //该运算传入MortanCode的副本，不改变MortanCode的大小
        }
        break;
    case direction_set::LEFT:
        bit = 1;
        reversebit(MortanCode, bit);
        threebits = getDatum(MortanCode);
        while (DATUM_LEFT.find(threebits) == DATUM_LEFT.end()) {
            if (++i >= depth) {
                return 0;
            }
            bit += 3;
            reversebit(MortanCode, bit);
            threebits = getDatum(MortanCode >> (bit-1)); //该运算传入MortanCode的副本，不改变MortanCode的大小
        }
        break;
    case direction_set::RIGHT:
        bit = 1;
        reversebit(MortanCode, bit);
        threebits = getDatum(MortanCode);
        while (DATUM_RIGHT.find(threebits) == DATUM_RIGHT.end()) {
            if (++i >= depth) {
                return 0;
            }
            bit += 3;
            reversebit(MortanCode, bit);
            threebits = getDatum(MortanCode >> (bit-1)); //该运算传入MortanCode的副本，不改变MortanCode的大小
        }
        break;
    default:
        std::cout << "The direction is illegal ! \n";
        break;
    }
    //根据该MortanCode对应节点的状态做相应处理
    OctreeNode* neighbor = LookupNode(MortanCode);
    if (neighbor != nullptr){
        if (neighbor->ChildExists == 0) {
            return MortanCode;           //存在此节点且为叶子节点，直接返回即可
        }
        else {                           // 存在此节点但是是枝节点，依照基准向找到符合范围的叶子节点
            return SearchNodeByPosition(MortanCode, incidentpoint);
        }
    }
    else {                              //不存在这个节点，寻找该Mortan码对应的父节点
        while (neighbor == nullptr && MortanCode >= 1) {
            MortanCode >>= 3;
            neighbor = LookupNode(MortanCode);
        }
        return MortanCode;
    }

}

uint64_t Octree::SearchNodeByPosition(uint64_t MortanCode, const Point* point) {
    int index = 0;
    OctreeNode* parent = LookupNode(MortanCode);
    for (int i = 0; i < 3; i++)
    {
        float delta = (*point)[i] - parent->m_center[i];
        //显然，对于我们这个定义，只有大于才会改变索引，故一个节点还包括他的右，上，前面
        if (delta > 0.0f)
            index |= (1 << i);
    }

    if (parent->ChildExists & (1 << index)) {
        // 点落在区域内且子节点不为空则递归的调用，一直找到该点所在的最小子块
        return SearchNodeByPosition((MortanCode << 3)| index, point);
    }
    else {
        //没有子节点，直接插入
        return MortanCode;
    }
}
uint64_t Octree::SearchNodeByPositionFromRoot(const Point* point,uint64_t MortanCode){
    int index = 0;
    OctreeNode* node = LookupNode(MortanCode);
    if(node == nullptr)
        return 0;
    for (int i = 0; i < 3; i++)
    {
        float delta = (*point)[i] - node->m_center[i];
        //显然，对于我们这个定义，只有大于才会改变索引，故一个节点还包括他的右，上，前面
        if (delta > 0.0f)
            index |= (1 << i);
    }

    if (node->ChildExists & (1 << index)) {
        // 点落在区域内且子节点不为空则递归的调用，一直找到该点所在的最小子块
        return SearchNodeByPosition((MortanCode << 3)| index, point);
    }
    else {
        //没有子节点，直接插入
        return MortanCode;
    }
}
//获得一个数字的后三位
uint64_t getDatum(uint64_t MortanCode) {
    uint64_t Datum = 0;
    for (int i = 0; i < 3; ++i) {
        Datum |= (getbit(MortanCode, i) << i);
    }
    return Datum;
}
OctreeNode* Octree::GetRoot(){
    return LookupNode(1);
}

